#include "headfile.h"

u8 circle_flag,square_flag,blank_flag;
u8 led_flag,delay_flag;

//circle_flag = 0;
//square_flag = 0;
//blank_flag = 0;
//led_flag = 0;
//delay_flag = 0;
/*-----------------------------------------------------------------------
        PB0左前   PA7左中     PA6左后     PA3右后     PA2右中     PA1右前
序号        1       2           3           4           5           6
以下为电机方向控制代码
------------------------------------------------------------------------*/
void Forward(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,x-15,0);
}

void TurnLeft(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,0,0);
}

void TurnRight(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,x-15,0);
}

void Left_PY(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,x-15,0);
}

void Right_PY(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,x,0);
    ESC_Speed_config(3,0,0);
}

void Diving(int x)
{
    ESC_Speed_config(5,x,1);
    ESC_Speed_config(2,x,1);
    ESC_Speed_config(4,120,0);      //maogugu
    ESC_Speed_config(3,105,0);      //maogugu
}

void Stop(void)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,0,0);
    ESC_Speed_config(5,0,1);
    ESC_Speed_config(2,0,1); 
}

void Rapidly_Right(int x)
{
    ESC_Speed_config(6,x,1);
    ESC_Speed_config(1,0,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,0,0);    
}

void Rapidly_Left(int x)
{
    ESC_Speed_config(6,0,1);
    ESC_Speed_config(1,x,0);
    ESC_Speed_config(4,0,0);
    ESC_Speed_config(3,0,0);    
}



/*-----------------------------------------------------------------------
红外传感器定深代码
(每个控制周期进入)
------------------------------------------------------------------------*/
void Dingshen(void)
{
    // if(HW1 && HW2 && HW3 && HW4 && HW5)
    if(Res[2] == 'Y')
        Diving(180);     //下潜（修改值）
    else
        Diving(0);    
}

/*-----------------------------------------------------------------------
红外传感器平移代码
（每隔n个周期进入一次）
------------------------------------------------------------------------*/
void Pingyi(void)
{
    if(HW1 + HW2 >= HW4 + HW5)
        Right_PY(150);  //右平移（修改值） 
    else if (HW1 + HW2 == HW4 + HW5)
        Forward(150);  //正常运行（修改值）  
    else if (HW1 + HW2 <= HW4 + HW5) 
        Left_PY(150);   //左平移 （修改值）

}

/*-----------------------------------------------------------------------
串口接收控制代码
目前仅设计了串口接收信息并控制姿态
------------------------------------------------------------------------*/
void USART_Data_Analysis(void)
{
    switch(Res[0])
    {
        case 'F':
            TurnLeft(50);   //左转（可修改））
            break;

        case 'D':
            TurnLeft(60);   //左转（可修改）
            break;

        case 'S':
            TurnLeft(70);   //左转（可修改）
            break;

        case 'A':
            Rapidly_Left(90);  //急左转（可修改）
            break;

        case 'H':
            TurnRight(50);  //右转（可修改）
            break;

        case 'J':
            TurnRight(60);   //右转（可修改）
            break;

        case 'K':
            TurnRight(70);  //右转（可修改）
            break;

        case 'L':
            Rapidly_Right(90);  //急右转（可修改）
            break;

        case 'G':
            Forward(75);    //前进（可修改）
            break;
				
				case 'Z':
            Left_PY(80);    //大左平移（可修改）
            break;
				
				case 'X':
            Left_PY(60);    //小左平移（可修改）
            break;
				
				case 'M':
            Right_PY(80);    //大右平移（可修改）
            break;
				
				case 'N':
            Right_PY(60);    //小右平移（可修改）
            break;
				
        case 'P':
            Stop();  
            break;
        default:
            break;
    }

	switch(Res[1])  
	{
		case 'C':
			circle_flag++;					
				break;
		case 'Q':
			square_flag++;
				break;
		default:
			blank_flag++;
				break;            
	}
    
    if( !led_flag )
    {
        if( circle_flag > Max(square_flag,blank_flag) )
        {
            LED1(1);LED2(0);LED3(0);
            led_flag = 1;
        }
        if( square_flag > Max(circle_flag,blank_flag))
        {
            LED1(0);LED2(1);LED3(0);
            led_flag = 1;
        }
        if( blank_flag > Max(circle_flag,square_flag))
        {
            LED1(0);LED2(0);LED3(0);
            led_flag = 1;
        }
    }
    else
    {
        delay_flag++;
        if (delay_flag >= 200)
        {
            delay_flag = 0;
            led_flag = 0;
            circle_flag = 0;
            square_flag = 0;
            blank_flag = 0;
        }
    }
}

void Init_Config(void)
{
	LED_Init_Config();
	EXTI_Init_Config();
	USART_Config();
	ESC_Strat_Init(); //最后初始化（内含定时器初始化）
    IWDG_Init();
}


/*
以下采集代码与控制代码均在中断中使用
*/
void Data_Collect(void)
{
		EXTI_ReadBit();
}


void control_Config(void)
{
	Data_Collect();
    USART_Data_Analysis();
	// Dingshen();	
    IDWG_Feed();

}



